/*:created on: Jun 19, 2012
 *      Author: wlin15
 */
#include "qrmcp.h"

gsl_matrix * qrmcp (gsl_matrix *B, gsl_vector *y, gsl_permutation *piv)
{

    printf("\nNow we are in qrmcp");
    int count=0;
	size_t m, n,m2,i;
	size_t j=0,k=0;
	m=B->size1;
	n=B->size2;
	m2=y->size;
	double rho,v1,tao;
			double ymv;
	if(m<n)
	{
		fprintf(stderr, "Matrix to be factorized is column-rank deficient!");
		exit(EXIT_FAILURE);
	}

	if(m2!=m)
	{
		fprintf(stderr, "Matrix and vector doesn't match");
		exit(EXIT_FAILURE);

	}
	printf("1: \n");

	printf("2: m=%zu, n=%zu, m2=%zu\n", m,n,m2);
	printf("3: A:\n");
	 for (i = 0; i < m; i++)
			 {
				 for (j = 0; j < n; j++)
				 {
					 printf("%g\t",gsl_matrix_get (B, i, j));

				 }
				 printf("\n");
			 }
	printf(":\n");
	printf("\n4: y:\n");
	gsl_vector_fprintf(stdout, y,"%g\t\t");
	printf("\n5: piv:\n");
	gsl_permutation_fprintf (stdout, piv, " %12u\n");


	gsl_matrix * colnormB=gsl_matrix_alloc(2,n);
	gsl_matrix_set_zero(colnormB);


	double d;
	gsl_vector_view colnormB_row1=gsl_matrix_row(colnormB,0);
	gsl_vector_view colnormB_row2=gsl_matrix_row(colnormB,1);

	for(j=0;j<n;j++)
	{
		gsl_vector_view colB=gsl_matrix_column(B,j);
		d=gsl_blas_dnrm2(&colB.vector);
		gsl_vector_set(&colnormB_row1.vector,j,d);
	}
	gsl_vector_mul(&colnormB_row1.vector,&colnormB_row1.vector);


	size_t minId,q;
	for(k=0;k<n-1;k++)
	{
		colnormB_row1=gsl_matrix_subrow(colnormB,0,k,n-k);
		colnormB_row2=gsl_matrix_subrow(colnormB,1,k,n-k);

		gsl_vector *temp=gsl_vector_alloc(n-k);
		gsl_vector_memcpy(temp, &colnormB_row1.vector);
		gsl_vector_sub(temp,&colnormB_row2.vector);
	/*	printf("\n 9:\n");
		printf("\n colnormB:\n");
			for (i = 0; i < 2; i++)
			{
				for (j = 0; j < n; j++)
				{
					printf("%e\t",gsl_matrix_get (colnormB, i, j));

				}
				printf("\n");
			}*/


		printf("The norm after subtraction in iteration %d is\n", count);

		for(i=0;i<temp->size;i++)
		{
			printf("%e\t",gsl_vector_get(temp,i));
		}

		minId=gsl_vector_min_index(temp);
		printf("\n 10:\n");
		printf("The min index is %zu \n", minId);
		q=minId+k;
		printf("k is %zu,q is %zu\n",k,q);
		if (minId>0)
		{
			gsl_permutation_swap(piv,k,q);

			gsl_vector_swap_elements(&colnormB_row1.vector,0,minId);
			gsl_vector_swap_elements(&colnormB_row2.vector,0,minId);
			gsl_matrix_swap_columns(B,k,q);
		}
        printf("Change the %zu th column and %zu th column of B\n", k,q);
		for(i=0;i<n;i++){
		printf("%zu\t",gsl_permutation_get(piv,i));
		}

		size_t s1;
		gsl_vector_view subCol=gsl_matrix_subcolumn(B,k,k+1,m-k-1);

		printf("\n 14:\n");
		printf("\n The %zuth to %zuth elements of %zuth column of B is \n",k+1,m-1,k);
		for(i=0;i<(&subCol.vector)->size;i++)
		{
			printf("%12g",gsl_vector_get(&subCol.vector,i));
		}

		printf("\nThe threshold is %g\n",gsl_blas_dnrm2(&subCol.vector));
		//if(gsl_blas_dnrm2(&subCol.vector)>1.19209290e-16F)
		if(gsl_blas_dnrm2(&subCol.vector)>0)
		{

		count++;

		printf("\n T Before transformation B is :\n");
		for (i = 0; i < m; i++)
		{
			for (j = 0; j < n; j++)
			{
				printf("%16g",gsl_matrix_get (B, i, j));

			}
			printf("\n");
		}
			gsl_vector_view vt=gsl_matrix_subcolumn(B,k,k,m-k);
			gsl_vector *v=gsl_vector_alloc(m-k);
			gsl_vector_memcpy(v,&vt.vector);
			rho=gsl_blas_dnrm2(v);
			v1=gsl_vector_get(v,0);
			if (v1>=0)
				rho=-rho;
			gsl_vector_set(v,0,v1-rho);
			tao=-1/(rho*gsl_vector_get(v,0));
			printf("rho is %g\n", rho);
			printf("tao is %f\n", tao);

			printf("k is %zu, v is\n",k);
		    for(i=0;i<v->size;i++)
		    {
		    	printf("%g\t\t",gsl_vector_get(v,i));
		    }

			gsl_matrix_set(B,k,k,rho);
			gsl_matrix_view subMB=gsl_matrix_submatrix(B,k,k+1,m-k,n-k-1);
			printf("\nsubMB is\n");
			for(i=0;i<(&subMB.matrix)->size1;i++)
			{
				for(j=0;j<(&subMB.matrix)->size2;j++)
				{
					printf("%g\t",gsl_matrix_get(&subMB.matrix,i,j));
				}
				printf("\n");
			}


			gsl_vector *vn=gsl_vector_alloc(v->size-1);
			gsl_blas_dgemv (CblasTrans, 1, &subMB.matrix, v, 0, vn);
			gsl_blas_dger(-tao,v,vn, &subMB.matrix);

			gsl_vector_view suby=gsl_vector_subvector(y,k,m-k);



			gsl_blas_ddot(&suby.vector,v,&ymv);
			//printf("\nymv is ");
			//printf("%f\n",ymv);
			gsl_blas_daxpy(-tao*ymv,v,&suby.vector);
			printf("suby is\n");
		    for(i=0;i<v->size;i++)
		    {
		    	printf("%g\t\t",gsl_vector_get(&suby.vector,i));
		    }

			/*printf("\n In each iteration y is \n");
			gsl_vector_fprintf(stdout,y,"%g");*/


			gsl_vector_view subrowB=gsl_matrix_subrow(B,k,k+1,n-k-1);
			gsl_vector *subRowB=gsl_vector_alloc((&subrowB.vector)->size);
			gsl_vector_memcpy(subRowB, &subrowB.vector);
			printf("\n tag B is :\n");
					 for (i = 0; i < m; i++)
							 {
								 for (j = 0; j < n; j++)
								 {
									 printf("%16g",gsl_matrix_get (B, i, j));

								 }
								 printf("\n");
							 }


					printf("\n subRowB is the %zuth column to %zuth column of the %zuth row of B\n",k+1,n-1,k);

					for(i=0;i<subRowB->size;i++)
					{
						printf("%16g\t",gsl_vector_get(subRowB,i));
					}


			gsl_vector_mul(subRowB,subRowB);


			gsl_vector_view subRowcolnormB=gsl_matrix_subrow(colnormB,1,k+1,n-k-1);
			gsl_blas_daxpy(1,subRowB,&subRowcolnormB.vector);

            printf("\nsubRowcolnormB is\n");
			for(i=0;i<(&subRowcolnormB.vector)->size;i++)
			{
				printf(" %12g\t",gsl_vector_get(&subRowcolnormB.vector,i));
			}


			printf("\nAfter %d th transformation, B is\n",count);

			 for (i = 0; i < m; i++)
					 {
						 for (j = 0; j < n; j++)
						 {
							 printf("%12g",gsl_matrix_get (B, i, j));

						 }
						 printf("\n");
					 }
			gsl_vector_free(v);
			gsl_vector_free(subRowB);
			gsl_vector_free(vn);

		}
		else
			{
			 printf("You can stop here");}
		 gsl_vector_free(temp);

	}


v1=gsl_matrix_get(B,m-1,n-1);

if ((v1*v1)>0)
{
	rho=v1*v1;
	if (v1>0)
		rho=-rho;
	v1=v1-rho;
	tao=-1/(rho*v1);
	gsl_matrix_set(B,m-1,n-1,rho);
	double yy=gsl_vector_get(y,m-1);
	gsl_vector_set(y,m-1,yy-tao*v1*v1*yy);
	}
			     gsl_matrix_free(colnormB);
			   	 printf("\nhere2\n");
					printf("3: A:\n");
								 for (i = 0; i < m; i++)
										 {
											 for (j = 0; j < n; j++)
											 {
												 printf("%12g",gsl_matrix_get (B, i, j));

											 }
											 printf("\n");}
								 printf("\n4: y:\n");
								 	gsl_vector_fprintf(stdout, y,"%12g");
								 	printf("%d",count);
			   	 return(B);







	}


